package com.gitee.wsl.mathematics.complex.quaternion.ext


import com.gitee.wsl.mathematics.coordinate.d3.Coordinate3
import com.gitee.wsl.mathematics.vector.ext.dot
import com.gitee.wsl.mathematics.complex.quaternion.Quaternion
import com.gitee.wsl.mathematics.coordinate.d3.MutableCoordinate3
import com.gitee.wsl.mathematics.number.compareTo
import com.gitee.wsl.unit.angle.AngleUnit
import com.gitee.wsl.unit.angle.ext.arcCosine
import com.gitee.wsl.unit.angle.ext.cos
import com.gitee.wsl.unit.angle.ext.sin
import com.gitee.wsl.unit.angle.ext.times

fun<T:Number,Q: Quaternion<T, Q>,C:Coordinate3<T, *>> C.rotated(q: Q): C{
    val r = q.create(-q.x, -q.y, -q.z, q.w)
    r.multiplyFront(this)
    r.multiplyFront(q)
    return q.createVec3(r.x, r.y, r.z) as C
}

fun<T:Number> MutableCoordinate3<T, *>.rotate(quaternion: Quaternion<T,*>){

    val w2 = quaternion.w * quaternion.w
    val x2 = quaternion.x * quaternion.x
    val y2 = quaternion.y * quaternion.y
    val z2 = quaternion.z * quaternion.z
    val zw = quaternion.z * quaternion.w
    val xy = quaternion.x * quaternion.y
    val xz = quaternion.x * quaternion.z
    val yw = quaternion.y * quaternion.w
    val yz = quaternion.y * quaternion.z
    val xw = quaternion.x * quaternion.w

    val m00 = w2 + x2 - z2 - y2
    val m01 = xy + zw + zw + xy
    val m02 = xz - yw + xz - yw
    val m10 = -zw + xy - zw + xy
    val m11 = y2 - z2 + w2 - x2
    val m12 = yz + yz + xw + xw
    val m20 = yw + xz + xz + yw
    val m21 = yz + yz - xw - xw
    val m22 = z2 - y2 - x2 + w2

    set(
        x = m00 * x + m10 * y + m20 * z,
        y = m01 * x + m11 * y + m21 * z,
        z = m02 * x + m12 * y + m22 * z,
    )
}

fun<T:Number> Quaternion<T, *>.angleBetween(b: Quaternion<T, *>): AngleUnit {
    val dot = this dot b
    return AngleUnit.arcCosine(2 * (dot * dot).toDouble() - 1)
}

fun<T:Number> AngleUnit.Companion.between(a: Quaternion<T, *>, b: Quaternion<T, *>) = a.angleBetween( b)

/**
 * Calculate a quaternion that the specified quaternion rotate around X axis.
 * @param quaternion - The specified quaternion
 * @param rad - The rotation angle in radians
 */
fun<T:Number> Quaternion<T, *>.rotateX(rad:AngleUnit) {
    val (_x, _y, _z, _w) = this
    val rad1 = rad * 0.5
    val bx = rad1.sin()
    val bw = rad1.cos()

    x = _x * bw + _w * bx
    y = _y * bw + _z * bx
    z = _z * bw - _y * bx
    w = _w * bw - _x * bx
}

/**
 * Calculate a quaternion that the specified quaternion rotate around X axis.
 * @param quaternion - The specified quaternion
 * @param rad - The rotation angle in radians
 */
fun<T:Number> Quaternion<T, *>.rotateY(rad:AngleUnit) {
    val (_x, _y, _z, _w) = this
    val rad1 = rad * 0.5
    val by = rad1.sin()
    val bw = rad1.cos()

    x = _x * bw - _z * by
    y = _y * bw + _w * by
    z = _z * bw + _x * by
    w = _w * bw - _y * by
}

/**
 * Calculate a quaternion that the specified quaternion rotate around X axis.
 * @param quaternion - The specified quaternion
 * @param rad - The rotation angle in radians
 */
fun<T:Number> Quaternion<T, *>.rotateZ(rad:AngleUnit) {
    val (_x, _y, _z, _w) = this
    val rad1 = rad * 0.5
    val bz = rad1.sin()
    val bw = rad1.cos()

    x = _x * bw + _y * bz
    y = _y * bw - _x * bz
    z = _z * bw + _w * bz
    w = _w * bw - _z * bz
}


/**
 * Returns the result of rotating toward [to] by [delta] (in radians). Passing a negative [delta] will rotate toward the inverse of [to].
 *
 * **Note:** Both quaternions must be normalized.
 */
fun<T:Number> Quaternion<T, *>.rotateToward(to: Quaternion<T, *>, delta: Double): Quaternion<T, *> {
    val unsignedDelta: Double
    val unsignedTo: Quaternion<T, *>

    if (delta < 0.0) {
        unsignedDelta = -delta
        unsignedTo = to.conjugate as Quaternion<T, *>
    } else {
        unsignedDelta = delta
        unsignedTo = to
    }

    val angle = angleTo(unsignedTo)

    return if (angle < unsignedDelta) {
        unsignedTo
    } else {
        slerp(unsignedTo, unsignedDelta / angle)
    }
}
